%author: wxj233
%time: 2023.8.31 12:00

clc;
clear;
close all;


data = SourceData('RadarData.csv');  % 数据预处理对象

% var_yr, var_xr, var_vr, q, L_door, speed_door, turn_v, V_threshold, life_max
tracer = Tracer(1.5, 5, 0.0277, 0.01, 0.99, 2.5, 8.33, 35, 3);  % 初始化跟踪器

count = [];
frames = [];

% data.setIndex(1000);
frame = data.getNextFrame();
while  ~isempty(frame)
    % frame, dyRcsMin, stRcsMin, y_left, y_right
    [staticPoints, movingPoints, notNullP] = data.speedRcsFilter(frame, -2, -10, -8, 8);  % 利用速度和RCS初步筛选点
    if isempty(movingPoints)
        movingPoints = notNullP;
    end
    % points, epsilon, minpts, zoomX, zoomY, zoomV
    [Eq_points, labels] = data.getEquivalentPoint(movingPoints, 7, 2, 0.2, 1, 10);  % 单帧的等效点,动态点聚类
    tracer.loadPoints(Eq_points);  % 加载点迹

%     shownum = 20;  % 显示帧数量
%     p_num = 1;  % 点的数量
%     if length(count) >= p_num
%         frames(1:count(1), :) = [];
%         count(1) = [];
%     end
%     frames = [frames;Eq_points];
%     count = [count, size(Eq_points, 1)];
% 
%     traksNum = length(tracer.tracks);
% 
%     figure(1);
%     gscatter(movingPoints(:,4), movingPoints(:,3), movingPoints(:,5));
%     hold on;
%     scatter(frames(:,4), frames(:,3),10, frames(:,5),'DisplayName',cat(2, '序列号',num2str(tracer.frame_num)));
%     hold on;
%     for i = 1:1:traksNum
%         len = size(tracer.tracks(i).points, 1);
%         if tracer.tracks(i).memNum/len > 1/2
%             continue;
%         end
% 
%         if len<shownum
%             plot(tracer.tracks(i).points(:,4), tracer.tracks(i).points(:,3),'DisplayName',cat(2, 'track',num2str(tracer.tracks(i).ID)));
%         else
%             plot(tracer.tracks(i).points(end-shownum+1:end,4), tracer.tracks(i).points(end-shownum+1:end,3));
%         end
% %         te = "T"+num2str(tracer.tracks(i).ID)+" x:"+num2str(tracer.tracks(i).points(end, 4)) + "y:"+num2str(tracer.tracks(i).points(end, 2));
%         te = "Track-"+num2str(tracer.tracks(i).ID);
%         text(tracer.tracks(i).points(end,4), tracer.tracks(i).points(end,3), te);
%         
%         hold on;
%     end
%     hold off;
%     axis([-250 250 0 500]);
%     title('毫米波雷达跟踪轨迹');
%     xlabel("横向范围/m");
%     ylabel("纵向范围/m");
%     legend;
%     set(gca,'XDir','reverse'); 
%     grid on;

    disp(data.index);
    tracer.calculate();  % 计算新的状态并更新
    frame = data.getNextFrame();
end
tracer.finish(); % 跟踪完成
tracer.classify(); % 目标分类
tracer.eventJudge();  % 事件判断
tracer.criterionOutput(15);  % 输出为标准格式


